#include "bayer2rgb.h"
#include <string.h>
#include <QDebug>


#define CLIP(in, out)\
   in = in < 0 ? 0 : in;\
   in = in > 255 ? 255 : in;\
   out = in;

inline void ClearBorders(uint8_t *rgb, int sx, int sy, int w)
{
    int i, j;
    // black edges are added with a width w:
    i = 3 * sx * w - 1;
    j = 3 * sx * sy - 1;
    while (i >= 0)
    {
        rgb[i--] = 0;
        rgb[j--] = 0;
    }
    i = sx * (sy - 1) * 3 - 1 + w * 3;
    while (i > sx)
    {
        j = 6 * w;
        while (j > 0)
        {
            rgb[i--] = 0;
            j--;
        }
        i -= (sx - 2 * w) * 3;
    }
}

/**************************************************************
 *     Color conversion functions for cameras that can        *
 * output raw-Bayer pattern images, such as some Basler and   *
 * Point Grey camera. Most of the algos presented here come   *
 * from http://www-ise.stanford.edu/~tingchen/ and have been  *
 * converted from Matlab to C and extended to all elementary  *
 * patterns.                                                  *
 **************************************************************/

/* 8-bits versions */
/* insprired by OpenCV's Bayer decoding */


/**
 * @brief bayer2rgb32_NearestNeighbor Bayer 格式转换为 RGB32 格式
 * @param bayer Bayer 格式数据
 * @param rgb32  #ARGB 格式排列(注意与 rgb24 格式的像素排列是相反的)
 * @param sx  width
 * @param sy  height
 * @param tile
 */
void bayer_to_RGB32_NearestNeighbor(const uint8_t *bayer, uint32_t *rgb32, int sx, int sy, BAYER_PATTERN tile)
{
    int height = sy;
    int blue = (tile == COLOR_FILTER_BGGR || tile == COLOR_FILTER_GBRG) ? -1 : 1;
    int start_with_green = (tile == COLOR_FILTER_GBRG || tile == COLOR_FILTER_GRBG);
    int i, imax;

    /* add black border */
    imax = sx * sy;

    uint8_t *pRGB32 = (uint8_t *) rgb32;
    pRGB32 += 1; //错开一个字节，相当于以 G 像素为中心
    height -= 1; //最后一行要单独处理
    while (height)
    {
        const uint8_t *bayerEnd = bayer + sx; //一行 bayer 像素的结束位置

        if (start_with_green)
        {
            pRGB32[blue] = bayer[1];          //blue == -1 对应 GBRG，这时 rgb24[1] = bayer[1] = B
            pRGB32[0] = bayer[sx + 1];  //G
            pRGB32[-blue] = bayer[sx];
            pRGB32[2] = 0;  //Alpha Channel
            bayer++;
            pRGB32 += 4;
        }

        if (blue > 0)// RGGB 当前行没有蓝色,不能是 GRBG 因为 start_with_green 处理完了
        {
            // RGRGRG
            // GBGBGB
            for (; bayer <= bayerEnd - 2; bayer += 2, pRGB32 += 8)
            {
                pRGB32[1] = bayer[0];            //R
                pRGB32[0] = bayer[1];             //G
                pRGB32[-1] = bayer[sx + 1]; //B
                pRGB32[2] = 0;  //Alpha Channel

                pRGB32[5] = bayer[2];             //R
                pRGB32[4] = bayer[sx + 2]; //G
                pRGB32[3] = bayer[sx + 1]; //B
                pRGB32[6] = 0;  //Alpha Channel
            }
        }
        else
        {
            // blue == -1 当前行有蓝色
            // BGBGBGBG
            // GRGRGRGR
            for (; bayer <= bayerEnd - 2; bayer += 2, pRGB32 += 8)
            {
                pRGB32[-1] = bayer[0];               //B
                pRGB32[0] = bayer[1];               //G
                pRGB32[1] = bayer[sx + 1];  //R
                pRGB32[2] = 0;  //Alpha Channel

                pRGB32[3] = bayer[2];               //B
                pRGB32[4] = bayer[sx + 2];   //G
                pRGB32[5] = bayer[sx + 1];   //R
                pRGB32[6] = 0;  //Alpha Channel
            }
        }

        if (bayer < bayerEnd)
        {
            pRGB32[blue] = bayer[0];
            pRGB32[0] = bayer[1];   //G
            pRGB32[-blue] = bayer[sx + 1];
            pRGB32[2] = 0;  //Alpha Channel
            bayer++;
            pRGB32 += 4;
        }

        blue = -blue;
        start_with_green = !start_with_green;
        --height;
    }
    //    for (i = (sx - 1); i < imax; i += sx)
    //    {
    //        rgb32[i] = rgb32[i - 1];
    //    }
    for (i = sx * (sy - 1); i < imax; i++)
    {
        rgb32[i] = rgb32[i - sx];
    }
}

/**************************************************************
 *     Color conversion functions for cameras that can        *
 * output raw-Bayer pattern images, such as some Basler and   *
 * Point Grey camera. Most of the algos presented here come   *
 * from http://www-ise.stanford.edu/~tingchen/ and have been  *
 * converted from Matlab to C and extended to all elementary  *
 * patterns.                                                  *
 **************************************************************/

/* 8-bits versions */
/* insprired by OpenCV's Bayer decoding */

void bayer_to_RGB24_NearestNeighbor(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    const int bayerStep = sx;
    const int rgbStep = 3 * sx;
    int width = sx;
    int height = sy;
    int blue = tile == COLOR_FILTER_BGGR || tile == COLOR_FILTER_GBRG ? -1 : 1;
    int start_with_green = tile == COLOR_FILTER_GBRG || tile == COLOR_FILTER_GRBG;
    int i, imax, iinc;

    /* add black border */
    imax = sx * sy * 3;
    for (i = sx * (sy - 1) * 3; i < imax; i++)
    {
        rgb24[i] = 0;
    }
    iinc = (sx - 1) * 3;
    for (i = (sx - 1) * 3; i < imax; i += iinc)
    {
        rgb24[i++] = 0;
        rgb24[i++] = 0;
        rgb24[i++] = 0;
    }

    rgb24 += 1;
    width -= 1;
    height -= 1;

    for (; height--; bayer += bayerStep, rgb24 += rgbStep)
    {
        //int t0, t1;
        const uint8_t *bayerEnd = bayer + width;

        if (start_with_green)
        {
            rgb24[-blue] = bayer[1];
            rgb24[0] = bayer[bayerStep + 1];
            rgb24[blue] = bayer[bayerStep];
            bayer++;
            rgb24 += 3;
        }

        if (blue > 0)
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                rgb24[-1] = bayer[0];
                rgb24[0] = bayer[1];
                rgb24[1] = bayer[bayerStep + 1];

                rgb24[2] = bayer[2];
                rgb24[3] = bayer[bayerStep + 2];
                rgb24[4] = bayer[bayerStep + 1];
            }
        }
        else
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                rgb24[1] = bayer[0];
                rgb24[0] = bayer[1];
                rgb24[-1] = bayer[bayerStep + 1];

                rgb24[4] = bayer[2];
                rgb24[3] = bayer[bayerStep + 2];
                rgb24[2] = bayer[bayerStep + 1];
            }
        }

        if (bayer < bayerEnd)
        {
            rgb24[-blue] = bayer[0];
            rgb24[0] = bayer[1];
            rgb24[blue] = bayer[bayerStep + 1];
            bayer++;
            rgb24 += 3;
        }

        bayer -= width;
        rgb24 -= width * 3;

        blue = -blue;
        start_with_green = !start_with_green;
    }
}

/* OpenCV's Bayer decoding */
void bayer_to_RGB24_Bilinear(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    const int bayerStep = sx;
    const int rgbStep = 3 * sx;
    int width = sx;
    int height = sy;
    /*
       the two letters  of the OpenCV name are respectively
       the 4th and 3rd letters from the blinky name,
       and we also have to switch R and B (OpenCV is BGR)

       CV_BayerBG2BGR <-> COLOR_FILTER_BGGR
       CV_BayerGB2BGR <-> COLOR_FILTER_GBRG
       CV_BayerGR2BGR <-> COLOR_FILTER_GRBG

       int blue = tile == CV_BayerBG2BGR || tile == CV_BayerGB2BGR ? -1 : 1;
       int start_with_green = tile == CV_BayerGB2BGR || tile == CV_BayerGR2BGR;
     */
    int blue = tile == COLOR_FILTER_BGGR || tile == COLOR_FILTER_GBRG ? -1 : 1;
    int start_with_green = tile == COLOR_FILTER_GBRG || tile == COLOR_FILTER_GRBG;

    ClearBorders(rgb24, sx, sy, 1);
    rgb24 += rgbStep + 3 + 1;
    height -= 2;
    width -= 2;

    for (; height--; bayer += bayerStep, rgb24 += rgbStep)
    {
        int t0, t1;
        const uint8_t *bayerEnd = bayer + width;

        if (start_with_green)
        {
            /* OpenCV has a bug in the next line, which was
           t0 = (bayer[0] + bayer[bayerStep * 2] + 1) >> 1; */
            t0 = (bayer[1] + bayer[bayerStep * 2 + 1] + 1) >> 1;
            t1 = (bayer[bayerStep] + bayer[bayerStep + 2] + 1) >> 1;
            rgb24[-blue] = (uint8_t) t0;
            rgb24[0] = bayer[bayerStep + 1];
            rgb24[blue] = (uint8_t) t1;
            bayer++;
            rgb24 += 3;
        }

        if (blue > 0)
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                t0 = (bayer[0] + bayer[2] + bayer[bayerStep * 2] + bayer[bayerStep * 2 + 2] + 2) >> 2;
                t1 = (bayer[1] + bayer[bayerStep] + bayer[bayerStep + 2] + bayer[bayerStep * 2 + 1] + 2) >> 2;
                rgb24[-1] = (uint8_t) t0;
                rgb24[0] = (uint8_t) t1;
                rgb24[1] = bayer[bayerStep + 1];

                t0 = (bayer[2] + bayer[bayerStep * 2 + 2] + 1) >> 1;
                t1 = (bayer[bayerStep + 1] + bayer[bayerStep + 3] + 1) >> 1;
                rgb24[2] = (uint8_t) t0;
                rgb24[3] = bayer[bayerStep + 2];
                rgb24[4] = (uint8_t) t1;
            }
        }
        else
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                t0 = (bayer[0] + bayer[2] + bayer[bayerStep * 2] + bayer[bayerStep * 2 + 2] + 2) >> 2;
                t1 = (bayer[1] + bayer[bayerStep] + bayer[bayerStep + 2] + bayer[bayerStep * 2 + 1] + 2) >> 2;
                rgb24[1] = (uint8_t) t0;
                rgb24[0] = (uint8_t) t1;
                rgb24[-1] = bayer[bayerStep + 1];

                t0 = (bayer[2] + bayer[bayerStep * 2 + 2] + 1) >> 1;
                t1 = (bayer[bayerStep + 1] + bayer[bayerStep + 3] + 1) >> 1;
                rgb24[4] = (uint8_t) t0;
                rgb24[3] = bayer[bayerStep + 2];
                rgb24[2] = (uint8_t) t1;
            }
        }

        if (bayer < bayerEnd)
        {
            t0 = (bayer[0] + bayer[2] + bayer[bayerStep * 2] + bayer[bayerStep * 2 + 2] + 2) >> 2;
            t1 = (bayer[1] + bayer[bayerStep] + bayer[bayerStep + 2] + bayer[bayerStep * 2 + 1] + 2) >> 2;
            rgb24[-blue] = (uint8_t) t0;
            rgb24[0] = (uint8_t) t1;
            rgb24[blue] = bayer[bayerStep + 1];
            bayer++;
            rgb24 += 3;
        }

        bayer -= width;
        rgb24 -= width * 3;

        blue = -blue;
        start_with_green = !start_with_green;
    }
}

/* High-Quality Linear Interpolation For Demosaicing Of
   Bayer-Patterned Color Images, by Henrique S. Malvar, Li-wei He, and
   Ross Cutler, in ICASSP'04 */
void bayer_to_RGB24_HQLinear(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    const int bayerStep = sx;
    const int rgbStep = 3 * sx;
    int width = sx;
    int height = sy;
    int blue = tile == COLOR_FILTER_BGGR || tile == COLOR_FILTER_GBRG ? -1 : 1;
    int start_with_green = tile == COLOR_FILTER_GBRG || tile == COLOR_FILTER_GRBG;

    ClearBorders(rgb24, sx, sy, 2);
    rgb24 += 2 * rgbStep + 6 + 1;
    height -= 4;
    width -= 4;

    /* We begin with a (+1 line,+1 column) offset with respect to bilinear decoding, so start_with_green is the same, but blue is opposite */
    blue = -blue;

    for (; height--; bayer += bayerStep, rgb24 += rgbStep)
    {
        int t0, t1;
        const uint8_t *bayerEnd = bayer + width;
        const int bayerStep2 = bayerStep * 2;
        const int bayerStep3 = bayerStep * 3;
        const int bayerStep4 = bayerStep * 4;

        if (start_with_green)
        {
            /* at green pixel */
            rgb24[0] = bayer[bayerStep2 + 2];
            t0 = rgb24[0] * 5
                    + ((bayer[bayerStep + 2] + bayer[bayerStep3 + 2]) << 2)
                    - bayer[2]
                    - bayer[bayerStep + 1]
                    - bayer[bayerStep + 3]
                    - bayer[bayerStep3 + 1]
                    - bayer[bayerStep3 + 3]
                    - bayer[bayerStep4 + 2]
                    + ((bayer[bayerStep2] + bayer[bayerStep2 + 4] + 1) >> 1);
            t1 = rgb24[0] * 5 +
                    ((bayer[bayerStep2 + 1] + bayer[bayerStep2 + 3]) << 2)
                    - bayer[bayerStep2]
                    - bayer[bayerStep + 1]
                    - bayer[bayerStep + 3]
                    - bayer[bayerStep3 + 1]
                    - bayer[bayerStep3 + 3]
                    - bayer[bayerStep2 + 4]
                    + ((bayer[2] + bayer[bayerStep4 + 2] + 1) >> 1);
            t0 = (t0 + 4) >> 3;
            CLIP(t0, rgb24[-blue]);
            t1 = (t1 + 4) >> 3;
            CLIP(t1, rgb24[blue]);
            bayer++;
            rgb24 += 3;
        }

        if (blue > 0)
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                /* B at B */
                rgb24[1] = bayer[bayerStep2 + 2];
                /* R at B */
                t0 = ((bayer[bayerStep + 1] + bayer[bayerStep + 3] +
                        bayer[bayerStep3 + 1] + bayer[bayerStep3 + 3]) << 1)
                        -
                        (((bayer[2] + bayer[bayerStep2] +
                          bayer[bayerStep2 + 4] + bayer[bayerStep4 +
                        2]) * 3 + 1) >> 1)
                        + rgb24[1] * 6;
                /* G at B */
                t1 = ((bayer[bayerStep + 2] + bayer[bayerStep2 + 1] +
                        bayer[bayerStep2 + 3] + bayer[bayerStep3 + 2]) << 1)
                        - (bayer[2] + bayer[bayerStep2] +
                        bayer[bayerStep2 + 4] + bayer[bayerStep4 + 2])
                        + (rgb24[1] << 2);
                t0 = (t0 + 4) >> 3;
                CLIP(t0, rgb24[-1]);
                t1 = (t1 + 4) >> 3;
                CLIP(t1, rgb24[0]);
                /* at green pixel */
                rgb24[3] = bayer[bayerStep2 + 3];
                t0 = rgb24[3] * 5
                        + ((bayer[bayerStep + 3] + bayer[bayerStep3 + 3]) << 2)
                        - bayer[3]
                        - bayer[bayerStep + 2]
                        - bayer[bayerStep + 4]
                        - bayer[bayerStep3 + 2]
                        - bayer[bayerStep3 + 4]
                        - bayer[bayerStep4 + 3]
                        +
                        ((bayer[bayerStep2 + 1] + bayer[bayerStep2 + 5] +
                        1) >> 1);
                t1 = rgb24[3] * 5 +
                        ((bayer[bayerStep2 + 2] + bayer[bayerStep2 + 4]) << 2)
                        - bayer[bayerStep2 + 1]
                        - bayer[bayerStep + 2]
                        - bayer[bayerStep + 4]
                        - bayer[bayerStep3 + 2]
                        - bayer[bayerStep3 + 4]
                        - bayer[bayerStep2 + 5]
                        + ((bayer[3] + bayer[bayerStep4 + 3] + 1) >> 1);
                t0 = (t0 + 4) >> 3;
                CLIP(t0, rgb24[2]);
                t1 = (t1 + 4) >> 3;
                CLIP(t1, rgb24[4]);
            }
        }
        else
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                /* R at R */
                rgb24[-1] = bayer[bayerStep2 + 2];
                /* B at R */
                t0 = ((bayer[bayerStep + 1] + bayer[bayerStep + 3] +
                        bayer[bayerStep3 + 1] + bayer[bayerStep3 + 3]) << 1)
                        -
                        (((bayer[2] + bayer[bayerStep2] +
                          bayer[bayerStep2 + 4] + bayer[bayerStep4 +
                        2]) * 3 + 1) >> 1)
                        + rgb24[-1] * 6;
                /* G at R */
                t1 = ((bayer[bayerStep + 2] + bayer[bayerStep2 + 1] +
                        bayer[bayerStep2 + 3] + bayer[bayerStep * 3 +
                        2]) << 1)
                        - (bayer[2] + bayer[bayerStep2] +
                        bayer[bayerStep2 + 4] + bayer[bayerStep4 + 2])
                        + (rgb24[-1] << 2);
                t0 = (t0 + 4) >> 3;
                CLIP(t0, rgb24[1]);
                t1 = (t1 + 4) >> 3;
                CLIP(t1, rgb24[0]);

                /* at green pixel */
                rgb24[3] = bayer[bayerStep2 + 3];
                t0 = rgb24[3] * 5
                        + ((bayer[bayerStep + 3] + bayer[bayerStep3 + 3]) << 2)
                        - bayer[3]
                        - bayer[bayerStep + 2]
                        - bayer[bayerStep + 4]
                        - bayer[bayerStep3 + 2]
                        - bayer[bayerStep3 + 4]
                        - bayer[bayerStep4 + 3]
                        +
                        ((bayer[bayerStep2 + 1] + bayer[bayerStep2 + 5] +
                        1) >> 1);
                t1 = rgb24[3] * 5 +
                        ((bayer[bayerStep2 + 2] + bayer[bayerStep2 + 4]) << 2)
                        - bayer[bayerStep2 + 1]
                        - bayer[bayerStep + 2]
                        - bayer[bayerStep + 4]
                        - bayer[bayerStep3 + 2]
                        - bayer[bayerStep3 + 4]
                        - bayer[bayerStep2 + 5]
                        + ((bayer[3] + bayer[bayerStep4 + 3] + 1) >> 1);
                t0 = (t0 + 4) >> 3;
                CLIP(t0, rgb24[4]);
                t1 = (t1 + 4) >> 3;
                CLIP(t1, rgb24[2]);
            }
        }

        if (bayer < bayerEnd)
        {
            /* B at B */
            rgb24[blue] = bayer[bayerStep2 + 2];
            /* R at B */
            t0 = ((bayer[bayerStep + 1] + bayer[bayerStep + 3] +
                    bayer[bayerStep3 + 1] + bayer[bayerStep3 + 3]) << 1)
                    -
                    (((bayer[2] + bayer[bayerStep2] +
                      bayer[bayerStep2 + 4] + bayer[bayerStep4 +
                    2]) * 3 + 1) >> 1)
                    + rgb24[blue] * 6;
            /* G at B */
            t1 = (((bayer[bayerStep + 2] + bayer[bayerStep2 + 1] +
                  bayer[bayerStep2 + 3] + bayer[bayerStep3 + 2])) << 1)
                    - (bayer[2] + bayer[bayerStep2] +
                    bayer[bayerStep2 + 4] + bayer[bayerStep4 + 2])
                    + (rgb24[blue] << 2);
            t0 = (t0 + 4) >> 3;
            CLIP(t0, rgb24[-blue]);
            t1 = (t1 + 4) >> 3;
            CLIP(t1, rgb24[0]);
            bayer++;
            rgb24 += 3;
        }

        bayer -= width;
        rgb24 -= width * 3;

        blue = -blue;
        start_with_green = !start_with_green;
    }
}

/* coriander's Bayer decoding (GPL) */
/* Edge Sensing Interpolation II from http://www-ise.stanford.edu/~tingchen/ */
/*   (Laroche,Claude A.  "Apparatus and method for adaptively
     interpolating a full color image utilizing chrominance gradients"
     U.S. Patent 5,373,322) */
void bayer_to_RGB24_EdgeSense(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    uint8_t *outR, *outG, *outB;
    register int i, j;
    int dh, dv;
    int tmp;

    // sx and sy should be even
    switch (tile)
    {
    case COLOR_FILTER_GRBG:
    case COLOR_FILTER_BGGR:
        outR = &rgb24[0];
        outG = &rgb24[1];
        outB = &rgb24[2];
        break;
    case COLOR_FILTER_GBRG:
    case COLOR_FILTER_RGGB:
        outR = &rgb24[2];
        outG = &rgb24[1];
        outB = &rgb24[0];
        break;
    default:
        //	fprintf(stderr, "Bad bayer pattern ID: %d\n", tile);
        //	return;
        break;
    }

    switch (tile)
    {
    case COLOR_FILTER_GRBG:	//---------------------------------------------------------
    case COLOR_FILTER_GBRG:
        // copy original RGB data to output images
        for (i = 0; i < sy*sx; i += (sx<<1))
        {
            for (j = 0; j < sx; j += 2)
            {
                outG[(i + j) * 3] = bayer[i + j];
                outG[(i + sx + (j + 1)) * 3] = bayer[i + sx + (j + 1)];
                outR[(i + j + 1) * 3] = bayer[i + j + 1];
                outB[(i + sx + j) * 3] = bayer[i + sx + j];
            }
        }
        // process GREEN channel
        for (i = 3*sx; i < (sy - 2)*sx; i += (sx<<1))
        {
            for (j = 2; j < sx - 3; j += 2)
            {
                dh = abs(((outB[(i + j - 2) * 3] +
                          outB[(i + j + 2) * 3]) >> 1) -
                        outB[(i + j) * 3]);
                dv = abs(((outB[(i - (sx<<1) + j) * 3] +
                          outB[(i + (sx<<1) + j) * 3]) >> 1)  -
                        outB[(i + j) * 3]);
                if (dh < dv)
                    tmp = (outG[(i + j - 1) * 3] +
                            outG[(i + j + 1) * 3]) >> 1;
                else
                {
                    if (dh > dv)
                        tmp = (outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 1;
                    else
                        tmp = (outG[(i + j - 1) * 3] +
                                outG[(i + j + 1) * 3] +
                                outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 2;
                }
                CLIP(tmp, outG[(i + j) * 3]);
            }
        }

        for (i = 2*sx; i < (sy - 3)*sx; i += (sx<<1))
        {
            for (j = 3; j < sx - 2; j += 2)
            {
                dh = abs(((outR[(i + j - 2) * 3] +
                          outR[(i + j + 2) * 3]) >>1 ) -
                        outR[(i + j) * 3]);
                dv = abs(((outR[(i - (sx<<1) + j) * 3] +
                          outR[(i + (sx<<1) + j) * 3]) >>1 ) -
                        outR[(i + j) * 3]);
                if (dh < dv)
                    tmp = (outG[(i + j - 1) * 3] +
                            outG[(i + j + 1) * 3]) >> 1;
                else
                {
                    if (dh > dv)
                        tmp = (outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 1;
                    else
                        tmp = (outG[(i + j - 1) * 3] +
                                outG[(i + j + 1) * 3] +
                                outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 2;
                }
                CLIP(tmp, outG[(i + j) * 3]);
            }
        }
        // process RED channel
        for (i = 0; i < (sy - 1)*sx; i += (sx<<1)) {
            for (j = 2; j < sx - 1; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outR[(i + j - 1) * 3] -
                         outG[(i + j - 1) * 3] +
                        outR[(i + j + 1) * 3] -
                        outG[(i + j + 1) * 3]) >> 1);
                CLIP(tmp, outR[(i + j) * 3]);
            }
        }
        for (i = sx; i < (sy - 2)*sx; i += (sx<<1)) {
            for (j = 1; j < sx; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outR[(i - sx + j) * 3] -
                         outG[(i - sx + j) * 3] +
                        outR[(i + sx + j) * 3] -
                        outG[(i + sx + j) * 3]) >> 1);
                CLIP(tmp, outR[(i + j) * 3]);
            }
            for (j = 2; j < sx - 1; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outR[(i - sx + j - 1) * 3] -
                         outG[(i - sx + j - 1) * 3] +
                        outR[(i - sx + j + 1) * 3] -
                        outG[(i - sx + j + 1) * 3] +
                        outR[(i + sx + j - 1) * 3] -
                        outG[(i + sx + j - 1) * 3] +
                        outR[(i + sx + j + 1) * 3] -
                        outG[(i + sx + j + 1) * 3]) >> 2);
                CLIP(tmp, outR[(i + j) * 3]);
            }
        }

        // process BLUE channel
        for (i = sx; i < sy*sx; i += (sx<<1)) {
            for (j = 1; j < sx - 2; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i + j - 1) * 3] -
                         outG[(i + j - 1) * 3] +
                        outB[(i + j + 1) * 3] -
                        outG[(i + j + 1) * 3]) >> 1);
                CLIP(tmp, outB[(i + j) * 3]);
            }
        }
        for (i = 2*sx; i < (sy - 1)*sx; i += (sx<<1)) {
            for (j = 0; j < sx - 1; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i - sx + j) * 3] -
                         outG[(i - sx + j) * 3] +
                        outB[(i + sx + j) * 3] -
                        outG[(i + sx + j) * 3]) >> 1);
                CLIP(tmp, outB[(i + j) * 3]);
            }
            for (j = 1; j < sx - 2; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i - sx + j - 1) * 3] -
                         outG[(i - sx + j - 1) * 3] +
                        outB[(i - sx + j + 1) * 3] -
                        outG[(i - sx + j + 1) * 3] +
                        outB[(i + sx + j - 1) * 3] -
                        outG[(i + sx + j - 1) * 3] +
                        outB[(i + sx + j + 1) * 3] -
                        outG[(i + sx + j + 1) * 3]) >> 2);
                CLIP(tmp, outB[(i + j) * 3]);
            }
        }
        break;

    case COLOR_FILTER_BGGR:	//---------------------------------------------------------
    case COLOR_FILTER_RGGB:
        // copy original RGB data to output images
        for (i = 0; i < sy*sx; i += (sx<<1)) {
            for (j = 0; j < sx; j += 2) {
                outB[(i + j) * 3] = bayer[i + j];
                outR[(i + sx + (j + 1)) * 3] = bayer[i + sx + (j + 1)];
                outG[(i + j + 1) * 3] = bayer[i + j + 1];
                outG[(i + sx + j) * 3] = bayer[i + sx + j];
            }
        }
        // process GREEN channel
        for (i = 2*sx; i < (sy - 2)*sx; i += (sx<<1)) {
            for (j = 2; j < sx - 3; j += 2) {
                dh = abs(((outB[(i + j - 2) * 3] +
                          outB[(i + j + 2) * 3]) >> 1) -
                        outB[(i + j) * 3]);
                dv = abs(((outB[(i - (sx<<1) + j) * 3] +
                          outB[(i + (sx<<1) + j) * 3]) >> 1) -
                        outB[(i + j) * 3]);
                if (dh < dv)
                    tmp = (outG[(i + j - 1) * 3] +
                            outG[(i + j + 1) * 3]) >> 1;
                else {
                    if (dh > dv)
                        tmp = (outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 1;
                    else
                        tmp = (outG[(i + j - 1) * 3] +
                                outG[(i + j + 1) * 3] +
                                outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >> 2;
                }
                CLIP(tmp, outG[(i + j) * 3]);
            }
        }
        for (i = 3*sx; i < (sy - 3)*sx; i += (sx<<1)) {
            for (j = 3; j < sx - 2; j += 2) {
                dh = abs(((outR[(i + j - 2) * 3] +
                          outR[(i + j + 2) * 3]) >> 1) -
                        outR[(i + j) * 3]);
                dv = abs(((outR[(i - (sx<<1) + j) * 3] +
                          outR[(i + (sx<<1) + j) * 3]) >> 1) -
                        outR[(i + j) * 3]);
                if (dh < dv)
                    tmp = (outG[(i + j - 1) * 3] +
                            outG[(i + j + 1) * 3]) >>1;
                else {
                    if (dh > dv)
                        tmp = (outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >>1;
                    else
                        tmp = (outG[(i + j - 1) * 3] +
                                outG[(i + j + 1) * 3] +
                                outG[(i - sx + j) * 3] +
                                outG[(i + sx + j) * 3]) >>2;
                }
                CLIP(tmp, outG[(i + j) * 3]);
            }
        }
        // process RED channel
        for (i = sx; i < (sy - 1)*sx; i += (sx<<1)) {	// G-points (1/2)
            for (j = 2; j < sx - 1; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outR[(i + j - 1) * 3] -
                         outG[(i + j - 1) * 3] +
                        outR[(i + j + 1) * 3] -
                        outG[(i + j + 1) * 3]) >>1);
                CLIP(tmp, outR[(i + j) * 3]);
            }
        }
        for (i = 2*sx; i < (sy - 2)*sx; i += (sx<<1)) {
            for (j = 1; j < sx; j += 2) {	// G-points (2/2)
                tmp = outG[(i + j) * 3] +
                        ((outR[(i - sx + j) * 3] -
                         outG[(i - sx + j) * 3] +
                        outR[(i + sx + j) * 3] -
                        outG[(i + sx + j) * 3]) >> 1);
                CLIP(tmp, outR[(i + j) * 3]);
            }
            for (j = 2; j < sx - 1; j += 2) {	// B-points
                tmp = outG[(i + j) * 3] +
                        ((outR[(i - sx + j - 1) * 3] -
                         outG[(i - sx + j - 1) * 3] +
                        outR[(i - sx + j + 1) * 3] -
                        outG[(i - sx + j + 1) * 3] +
                        outR[(i + sx + j - 1) * 3] -
                        outG[(i + sx + j - 1) * 3] +
                        outR[(i + sx + j + 1) * 3] -
                        outG[(i + sx + j + 1) * 3]) >> 2);
                CLIP(tmp, outR[(i + j) * 3]);
            }
        }

        // process BLUE channel
        for (i = 0; i < sy*sx; i += (sx<<1)) {
            for (j = 1; j < sx - 2; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i + j - 1) * 3] -
                         outG[(i + j - 1) * 3] +
                        outB[(i + j + 1) * 3] -
                        outG[(i + j + 1) * 3]) >> 1);
                CLIP(tmp, outB[(i + j) * 3]);
            }
        }
        for (i = sx; i < (sy - 1)*sx; i += (sx<<1)) {
            for (j = 0; j < sx - 1; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i - sx + j) * 3] -
                         outG[(i - sx + j) * 3] +
                        outB[(i + sx + j) * 3] -
                        outG[(i + sx + j) * 3]) >> 1);
                CLIP(tmp, outB[(i + j) * 3]);
            }
            for (j = 1; j < sx - 2; j += 2) {
                tmp = outG[(i + j) * 3] +
                        ((outB[(i - sx + j - 1) * 3] -
                         outG[(i - sx + j - 1) * 3] +
                        outB[(i - sx + j + 1) * 3] -
                        outG[(i - sx + j + 1) * 3] +
                        outB[(i + sx + j - 1) * 3] -
                        outG[(i + sx + j - 1) * 3] +
                        outB[(i + sx + j + 1) * 3] -
                        outG[(i + sx + j + 1) * 3]) >> 2);
                CLIP(tmp, outB[(i + j) * 3]);
            }
        }
        break;
    default:			//---------------------------------------------------------
        //      fprintf(stderr, "Bad bayer pattern ID: %d\n", tile);
        //      return;
        break;
    }

    ClearBorders(rgb24, sx, sy, 3);
}

/* coriander's Bayer decoding (GPL) */
void bayer_to_RGB24_Downsample(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    uint8_t *outR, *outG, *outB;
    register int i, j;
    int tmp;

    sx *= 2;
    sy *= 2;

    switch (tile)
    {
    case COLOR_FILTER_GRBG:
    case COLOR_FILTER_BGGR:
        outR = &rgb24[0];
        outG = &rgb24[1];
        outB = &rgb24[2];
        break;
    case COLOR_FILTER_GBRG:
    case COLOR_FILTER_RGGB:
        outR = &rgb24[2];
        outG = &rgb24[1];
        outB = &rgb24[0];
        break;
    default:
        //	fprintf(stderr, "Bad Bayer pattern ID: %d\n", tile);
        //	return;
        break;
    }

    switch (tile)
    {
    case COLOR_FILTER_GRBG:	//---------------------------------------------------------
    case COLOR_FILTER_GBRG:
        for (i = 0; i < sy*sx; i += (sx<<1))
        {
            for (j = 0; j < sx; j += 2)
            {
                tmp = ((bayer[i + j] + bayer[i + sx + j + 1]) >> 1);
                CLIP(tmp, outG[((i >> 2) + (j >> 1)) * 3]);
                tmp = bayer[i + sx + j + 1];
                CLIP(tmp, outR[((i >> 2) + (j >> 1)) * 3]);
                tmp = bayer[i + sx + j];
                CLIP(tmp, outB[((i >> 2) + (j >> 1)) * 3]);
            }
        }
        break;
    case COLOR_FILTER_BGGR:	//---------------------------------------------------------
    case COLOR_FILTER_RGGB:
        for (i = 0; i < sy*sx; i += (sx<<1))
        {
            for (j = 0; j < sx; j += 2)
            {
                tmp = ((bayer[i + sx + j] + bayer[i + j + 1]) >> 1);
                CLIP(tmp, outG[((i >> 2) + (j >> 1)) * 3]);
                tmp = bayer[i + sx + j + 1];
                CLIP(tmp, outR[((i >> 2) + (j >> 1)) * 3]);
                tmp = bayer[i + j];
                CLIP(tmp, outB[((i >> 2) + (j >> 1)) * 3]);
            }
        }
        break;
    default:			//---------------------------------------------------------
        //	fprintf(stderr, "Bad Bayer pattern ID: %d\n", tile);
        //	return;
        break;
    }

}

/* this is the method used inside AVT cameras. See AVT docs. */
void bayer_to_RGB24_Simple(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    const int bayerStep = sx;
    const int rgbStep = 3 * sx;
    int width = sx;
    int height = sy;
    int blue = tile == COLOR_FILTER_BGGR || tile == COLOR_FILTER_GBRG ? -1 : 1;
    int start_with_green = tile == COLOR_FILTER_GBRG || tile == COLOR_FILTER_GRBG;
    int i, imax, iinc;

    /* add black border */
    imax = sx * sy * 3;
    for (i = sx * (sy - 1) * 3; i < imax; i++)
    {
        rgb24[i] = 0;
    }
    iinc = (sx - 1) * 3;
    for (i = (sx - 1) * 3; i < imax; i += iinc)
    {
        rgb24[i++] = 0;
        rgb24[i++] = 0;
        rgb24[i++] = 0;
    }

    rgb24 += 1;
    width -= 1;
    height -= 1;

    for (; height--; bayer += bayerStep, rgb24 += rgbStep)
    {
        const uint8_t *bayerEnd = bayer + width;

        if (start_with_green)
        {
            rgb24[-blue] = bayer[1];
            rgb24[0] = (bayer[0] + bayer[bayerStep + 1] + 1) >> 1;
            rgb24[blue] = bayer[bayerStep];
            bayer++;
            rgb24 += 3;
        }

        if (blue > 0)
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                rgb24[-1] = bayer[0];
                rgb24[0] = (bayer[1] + bayer[bayerStep] + 1) >> 1;
                rgb24[1] = bayer[bayerStep + 1];

                rgb24[2] = bayer[2];
                rgb24[3] = (bayer[1] + bayer[bayerStep + 2] + 1) >> 1;
                rgb24[4] = bayer[bayerStep + 1];
            }
        }
        else
        {
            for (; bayer <= bayerEnd - 2; bayer += 2, rgb24 += 6)
            {
                rgb24[1] = bayer[0];
                rgb24[0] = (bayer[1] + bayer[bayerStep] + 1) >> 1;
                rgb24[-1] = bayer[bayerStep + 1];

                rgb24[4] = bayer[2];
                rgb24[3] = (bayer[1] + bayer[bayerStep + 2] + 1) >> 1;
                rgb24[2] = bayer[bayerStep + 1];
            }
        }

        if (bayer < bayerEnd)
        {
            rgb24[-blue] = bayer[0];
            rgb24[0] = (bayer[1] + bayer[bayerStep] + 1) >> 1;
            rgb24[blue] = bayer[bayerStep + 1];
            bayer++;
            rgb24 += 3;
        }

        bayer -= width;
        rgb24 -= width * 3;

        blue = -blue;
        start_with_green = !start_with_green;
    }
}

/**
 * @brief bayer_to_RGB24_None 直接把每个 Bayer 像素的值作为灰度赋值给 QImage 的每个像素
 * @param bayer
 * @param rgb24
 * @param sx
 * @param sy
 * @param tile
 */
void bayer_to_RGB24_None(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile)
{
    Q_UNUSED(tile);
    //qDebug() << "N";
    //const int rgbStep = 3 * sx;
    for (int i = 0; i < sy; i ++)
    {
        for(int j = 0; j < sx; j ++)
        {
//            rgb24[i * rgbStep + j * 3] = bayer[i * sx + j];
//            rgb24[i * rgbStep + j * 3 + 1] = bayer[i * sx + j];
//            rgb24[i * rgbStep + j * 3 + 2] = bayer[i * sx + j];

            *rgb24++ = *bayer;
            *rgb24++ = *bayer;
            *rgb24++ = *bayer++;
        }
    }
}

int bayer_to_RGB24_decoding_8bit(const uint8_t *bayer, uint8_t *rgb24, int sx, int sy, BAYER_PATTERN tile, BAYER_METHOD method)
{
    switch (method)
    {
    case BAYER_METHOD_NEAREST:
        bayer_to_RGB24_NearestNeighbor(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    case BAYER_METHOD_SIMPLE:
        bayer_to_RGB24_Simple(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    case BAYER_METHOD_BILINEAR:
        bayer_to_RGB24_Bilinear(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    case BAYER_METHOD_HQLINEAR:
        bayer_to_RGB24_HQLinear(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    case BAYER_METHOD_EDGESENSE:
        bayer_to_RGB24_EdgeSense(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    case BAYER_METHOD_NONE:
        bayer_to_RGB24_None(bayer, rgb24, sx, sy, tile);
        return BAYER_SUCCESS;
    }

    return INVALID_BAYER_METHOD;
}
